#include "LidarMultiDriverX.h"

#include "Driver/LidarDriverRS.h"
#include "Driver/LidarDriverHS.h"
#include "Driver/LidarDriverPCD.h"

#pragma region Init
LidarMultiDriverX::LidarMultiDriverX(int driver_count, std::vector<LidarConfigurationOptions *> options)
    : _driver_count(driver_count)
{
    if (options.size() != driver_count)
        _driver_count = options.size();

    if (_driver_count > _driver_max_count)
    {
        std::cout << "Support Max 4 Lidar Driver." << std::endl;
        return;
    }

    for (int i = 0; i < _driver_count; i++)
    {
        LidarDriverX *driver;
        LidarInputSourceType sourceType = std::get<LidarInputSourceType>(options[i]->GetValue(LidarConfigurationOptions::Data_Source_SrcType));
        if (sourceType == LidarInputSourceType::PCD_FILE)
            driver = new LidarDriverPCD(options[i], i);
        else
        {
            LidarManufacturerType manuType = std::get<LidarManufacturerType>(options[i]->GetValue(LidarConfigurationOptions::Data_SrcNet_ManuType));
            switch (manuType)
            {
            case LidarManufacturerType::RS_16:
            case LidarManufacturerType::RS_32:
            case LidarManufacturerType::RS_BP:
            case LidarManufacturerType::RS_HELIOS:
            case LidarManufacturerType::RS_128:
            case LidarManufacturerType::RS_80:
            case LidarManufacturerType::RS_M1:
                driver = new LidarDriverRS(options[i], i);
                break;
            case LidarManufacturerType::AT_128:
                driver = new LidarDriverHS(options[i], i);
                break;
            default:
                continue;
            }
        }
        driver->addPointCloudMsgCallback(std::bind(&LidarMultiDriverX::processData, this, std::placeholders::_1));

        _drivers[i] = driver;
        _options[i] = options[i];
    }
}

LidarMultiDriverX::~LidarMultiDriverX()
{
    stop();
    for (int i = 0; i < _driver_count; i++)
    {
        if (_drivers[i] != nullptr)
        {
            delete _drivers[i];
            _drivers[i] = nullptr;
            delete _options[i];
            _options[i] = nullptr;
        }
    }

    _pointCloudDataCallbacks.clear();
}

void LidarMultiDriverX::start()
{
    _to_exit_process = false;

    for (int i = 0; i < _driver_count; i++)
    {
        if (_drivers[i] == nullptr)
            continue;

        _drivers[i]->start();
    }

    _threadPointCloudProcess.setName("MultiPointCloudProcess");
    _threadPointCloudProcess.start(&LidarMultiDriverX::threadPointCloudProcess, this);
}

void LidarMultiDriverX::stop()
{
    for (int i = 0; i < _driver_count; i++)
    {
        if (_drivers[i] == nullptr)
            continue;

        _drivers[i]->stop();
    }

    _to_exit_process = true;
    _threadPointCloudProcess.tryJoin(5000);
}
#pragma endregion

#pragma region Process
void LidarMultiDriverX::threadPointCloudProcess(void *pContext)
{
    LidarMultiDriverX *pHandler = static_cast<LidarMultiDriverX *>(pContext);
    pHandler->processPointCloud();
}

void LidarMultiDriverX::processPointCloud()
{
    while (!_to_exit_process)
    {
        pcl::PointCloud<PointXYZI>::Ptr _pcl_pointcloud(new pcl::PointCloud<PointXYZI>);
        pcl::PointCloud<PointXYZI>::Ptr _temp_pointcloud(new pcl::PointCloud<PointXYZI>);
        PointCloudData<PointXYZI> _data;
        for (int i = 0; i < _driver_count; i++)
        {
            std::shared_ptr<PointCloudData<pcl::PointXYZI>> _msg = _stuffed_cloud_queue[i].popWait();
            if (_msg.get() != NULL)
            {
                _temp_pointcloud.swap(_msg->cloud_ptr);
                _pcl_pointcloud->insert(_pcl_pointcloud->end(), _temp_pointcloud->begin(), _temp_pointcloud->end());
                if (_data.timestamp < _msg->timestamp)
                    _data.timestamp = _msg->timestamp;
            }
        }
        _data.cloud_ptr = _pcl_pointcloud;

        for (PointCloudDataCallback handler : _pointCloudDataCallbacks)
        {
            handler(_data);
        }
    }
}
#pragma endregion

#pragma region Callback
bool LidarMultiDriverX::addMultiPointCloudMsgCallback(PointCloudDataCallback callback)
{
    _pointCloudDataCallbacks.push_back(callback);
    return true;
}

bool LidarMultiDriverX::processData(PointCloudData<pcl::PointXYZI> &data)
{
    int id = data.lidar_id;

    std::shared_ptr<PointCloudData<pcl::PointXYZI>> _data = std::make_shared<PointCloudData<pcl::PointXYZI>>();
    pcl::PointCloud<PointXYZI>::Ptr _cloud(new pcl::PointCloud<PointXYZI>);
    _data->cloud_ptr = _cloud;
    _data->cloud_ptr.swap(data.cloud_ptr);
    _data->timestamp = data.timestamp;

    auto len = _stuffed_cloud_queue[id].push(_data);
    if (len > _que_max_size)
        _stuffed_cloud_queue[id].pop();

    return true;
}
#pragma endregion